//
//  CharacterController.m
//  CompGraf2
//
//  Created by Juan Niosi on 04/10/11.
//  Copyright 2011 __MyCompanyName__. All rights reserved.
//

#import "CharacterController.h"
#import "Vector3.h"

@implementation CharacterController
@synthesize animHit, animAttack, animIdle, animWalk;

- (id)init:(MD2Model *)md2 withWalkSpeed:(GLfloat)walk withTurnSpeed:(GLfloat)turn :(MapNode *)pos
{
    self = [super init:md2 withAnim:@"stand"];
    if (self) {
        state = IDLE;
        movementQueue = [[NSMutableArray alloc] init];
        walkspeed = walk;
        turnspeed = turn;
        currentPos = pos;
        turning = true;
        animHit = @"hit";
        animIdle = @"stand";
        animWalk = @"run";
        animAttack = @"attack";
    }
    
    return self;
}

- (void)update:(float)deltaTime
{
    [super update:deltaTime];
    switch (state)
    {
        case IDLE:
            break;
            
        case WALKING:
            [self updateMove:deltaTime];
            break;
            
        default:
            break;
            
    }
}

- (void)setPath:(NSArray *)path
{
    if (path == NULL || [path count] == 0)
        return;
    
    if ([movementQueue count] > 1)
    {
        //Remove all but last
        MapNode *first = [movementQueue objectAtIndex:0];
        [movementQueue removeAllObjects];
        [movementQueue addObject:first];
    }
    for (int i = 1; i < [path count]; i++)
    {
        MapNode *nxt = [path objectAtIndex:i];
        [movementQueue addObject:nxt];
    }
    currentPos = [movementQueue objectAtIndex:0];
    [self setState:WALKING];
}

- (void)updateMove:(GLfloat)deltaTime
{
    if ([movementQueue count] == 0)
    {
        [self setState:IDLE];
        return;
    }
    
    if (currentPos == NULL)
    {
        currentPos = [movementQueue objectAtIndex:0];
        Vector3 *dist = [Vector3 subtract:currentPos.worldPos  :[self posToVec3]];
        if ([dist length2] < 0.1f)
        {
            [movementQueue removeObjectAtIndex:0];
            currentPos = [movementQueue objectAtIndex:0];
        }
    }
    
    Vector3 *dir = [Vector3 subtract:currentPos.worldPos  :[self posToVec3]];
    
    if (turning)
    {
        Vector3 *normalDir = [dir copy];
        [normalDir normalize];
        Vector3 *curDir = [Vector3 yRotationToVec3:self.rotation[1]];
        [curDir  normalize];
        float dot = [Vector3 dot:normalDir :curDir];
        float deltaRot = [Vector3 angle:normalDir :curDir];
        Vector3 *cx = [Vector3 cross:curDir :normalDir];
        float rot = turnspeed*deltaTime;

        if (dot < 0.9f)
        {
            if ([cx getY] < 0)
                rot *= -1.0f;
            [self rotateY:rot];
            return;
        }
        else
        {
            [self rotateY:deltaRot];
            turning = false;
        }
    }
    
    Vector3 *movement = [dir copy];
    [movement normalize];
    [movement scale:deltaTime*walkspeed];
    
    if ([dir length2] < [movement length2] + 0.01)
    {
        [self translateX:[dir getX]];
        [self translateZ:[dir getZ]];
        [movementQueue removeObjectAtIndex:0];
        turning = true;
        if ([movementQueue count] == 0)
        {
            [self setState:IDLE];
        }
        else
        {
            currentPos = [movementQueue objectAtIndex:0];
        }
    }
    else
    {
        [self translateX:[movement getX]];
        [self translateZ:[movement getZ]];
    }
    
}

- (MapNode *)getCurrentObjective
{
    return currentPos;    
}

- (BOOL)setState:(STATE)newState
{
    if (state == newState)
        return false;
    
    state = newState;
    
    switch (state) 
    {
        case IDLE:
            [self playAnim:@"stand" loop:true];
            break;
            
        case WALKING:
            [self playAnim:@"run" loop:true];
            break;
            
        default:
            break;
    }
        
    return true;
}

- (void)dealloc
{
    [movementQueue release];
    [animHit release];
    [animAttack release];
    [animIdle release];
    [animName release];
    [animWalk release];
    
    [super release];
}
@end
